#include "main.h"
#include "svpwm.h"
#include "arm_math.h"

q15_t Tmpa,Tmpb;
int16_t testBuff = 0;
void svpwmTheta2ab16(int16_t theta16, int16_t k, int16_t* alpha, int16_t* beta)
{
    //k = k*0.8660254;//Umax = Udc * (3^0.5)/2
    k = ((int32_t)k* 56756)/ 65536;

    testBuff = theta16;
    Tmpa = arm_cos_q15(theta16);
    Tmpb = arm_sin_q15(theta16);

    *alpha = (int16_t)((int32_t)Tmpa*k/0x8000);
    *beta  = (int16_t)((int32_t)Tmpb*k/0x8000);

}

void svpwmCal(int16_t alpha, int16_t beta, StPwmDuty_t* output)
{
    int8_t VecSector = 0;
    int16_t Ta = 0,Tb = 0,Tc = 0;

    //alpha,beta => a,b,c
    q15_t b = beta;                                         //b=beta
    int16_t temp;
    temp = ((int32_t)alpha* 56756)/ 65536;
    q15_t a = (temp) - (beta/2);   //a=alpha*sqrt(3)/2 - beta/2
    q15_t c = (-temp) - (beta/2);   //c=-alpha*sqrt(3)/2 - beta/2

    //check sector
    if(b > 0)
    {
        VecSector += 1;
    }
    if(a > 0)
    {
        VecSector += 2;
    }
    if(c > 0)
    {
        VecSector += 4;
    }

    //a,b,c => a',b',c'
    a += beta;  //a'= alpha*sqrt(3)/2 + beta/2
    c = b - a;  //c'= -alpha*sqrt(3)/2 + beta/2

    //duty calulate
    switch(VecSector)
    {
        case 0:
            Ta = (0x8000)/2;
            Tb = (0x8000)/2;
            Tc = (0x8000)/2;
            break;
        case 1:
            Tb = ((0x8000) - a - c)/2;
            Ta = Tb + c;
            Tc = Ta + a;
            break;
        case 2:
            Ta = ((0x8000) - a + b)/2;
            Tc = Ta + a;
            Tb = Tc - b;
            break;
        case 3:
            Ta = ((0x8000) + c - b)/2;
            Tb = Ta - c;
            Tc = Tb + b;
            break;
        case 4:
            Tc = ((0x8000) - c + b)/2;
            Tb = Tc - b;
            Ta = Tb + c;
            break;
        case 5:
            Tb = ((0x8000) + a - b)/2;
            Tc = Tb + b;
            Ta = Tc - a;
            break;
        case 6:
            Tc = ((0x8000) + a + c)/2;
            Ta = Tc - a;
            Tb = Ta - c;
            break;
        default:
            Ta = (0x8000)/2;
            Tb = (0x8000)/2;
            Tc = (0x8000)/2;
            break;
    }

    output->pwmDutyPresent[0] = Ta;
    output->pwmDutyPresent[1] = Tb;
    output->pwmDutyPresent[2] = Tc;
}



